#include "projector.h"
#include "sharedStructure.hpp"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <iostream>

#include <deque>

#include "Eigen/SVD"
#include "Eigen/Dense"


#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include "AR/ar.h"

bool projector::initialize(float s, float *p_R, float *p_T,
	 int num_orient, int num_length, int filter_size){
	// s
	this->s_wt = s;
	this->inv_s_wt = 1.0f/s;
	// R
	for( int r = 0; r<3; ++r){
		for (int c = 0; c<3; ++c){
			R[r][c] = p_R[c*3+r];
		}
	}
	// compute inv(R);
	invR();

	// T, 

	memcpy( this->T,p_T,sizeof(float)*3);

	//
	n_orient = num_orient;
	n_length = num_length;

	this->fh = filter_size;

	return true;

}

bool projector::release(){
/*
	free(cache_cos);
	free(cache_sin);
*/
	return true;
}



void projector::invR(){
	float det = R[0][0]*R[1][1]*R[2][2] + R[0][1]*R[1][2]*R[2][0] + R[1][0]*R[2][1]*R[0][2]
	- R[0][2]*R[1][1]*R[2][0] - R[0][0]*R[1][2]*R[2][1] - R[0][1]*R[1][0]*R[2][2];
	assert(fabs(det)>1e-10);
	inv_R[0][0]=R[1][1]*R[2][2]-R[1][2]*R[2][1];
	inv_R[1][0]=R[1][2]*R[2][0]-R[1][0]*R[2][2];
	inv_R[2][0]=R[1][0]*R[2][1]-R[1][1]*R[2][0];

	inv_R[0][1]=R[0][2]*R[2][1]-R[0][1]*R[2][2];
	inv_R[1][1]=R[0][0]*R[2][2]-R[0][2]*R[2][0];
	inv_R[2][1]=R[0][1]*R[2][0]-R[0][0]*R[2][1];

	inv_R[0][2]=R[0][1]*R[1][2]-R[0][2]*R[1][1];
	inv_R[1][2]=R[0][2]*R[1][0]-R[0][0]*R[1][2];
	inv_R[2][2]=R[0][0]*R[1][1]-R[0][1]*R[1][0];

	det = 1.0f/det;
	for (int r=0; r<3; ++r){
		for(int c = 0; c<3; ++c){
			inv_R[r][c]*=det;
		}
	}
	return;
}

bool projector::updatePlane(float *p_u, float *p_v, float *p_ori, float *p_nt, float s, float width, float height){

	memcpy(u,p_u, sizeof(float)*3);
	memcpy(v,p_v, sizeof(float)*3);
	float norm = u[0]*u[0]+u[1]*u[1]+u[2]*u[2];
	if( fabs(norm-1)>1e-6){
		printf("input bu is not unit vector: norm(bu)=%f\n",norm);
		u[0]/=norm;
		u[1]/=norm;
		u[2]/=norm;
	}

	norm = v[0]*v[0]+v[1]*v[1]+v[2]*v[2];
	if( fabs(norm-1)>1e-6){
		printf("input bv is not unit vector: norm(bv)=%f\n",norm);
		v[0]/=norm;
		v[1]/=norm;
		v[2]/=norm;
	}

	memcpy(nt,p_nt,sizeof(float)*4);
	norm =norm = nt[0]*nt[0]+nt[1]*nt[1]+nt[2]*nt[2];
	if( fabs(norm-1)>1e-6){
		printf("input nt is not unit vector: norm(nt)=%f\n",norm);
		nt[0]/=norm;
		nt[1]/=norm;
		nt[2]/=norm;
		nt[3]/=norm;
	}

	// convert the nt from tan coordiante system to world coordinate system
	tanDirection2World(nt[0],nt[1],nt[2],n[0],n[1],n[2]);

	memcpy(plane_origin, p_ori, sizeof(float)*3);

	this->s_tp = s;
	this->inv_s_tp = 1.0f/s;
	this->w = width; // size in plane coordinate unit, e.g. pixels
	this->h = height;


	computePwp();
	return true;
}	

bool projector::updateCamera(float *pP, float *direction, float zoom_x, float zoom_y){
	for( int r = 0; r<3; ++r){
		for (int c = 0; c<4; ++c){
			P[r][c]= pP[r+c*3];
		}
	}

	memcpy(cam_dir,direction,sizeof(float)*3);
	float norm = direction[0]*direction[0]+direction[1]*direction[1]+direction[2]*direction[2];
	if( fabs(norm-1)>1e-10){
		cam_dir[0]/=norm;
		cam_dir[1]/=norm;
		cam_dir[2]/=norm;
	}

	zx = zoom_x;
	zy = zoom_y;

	updatePreprocessedMatrix();

	return true;
}


bool projector::tanPoint2World(float Xt, float Yt, float Zt, float &Xw, float &Yw, float &Zw){
	// X^T = s*R*(X^W+T);
	// X^W = inv(s)inv(R) X^T -T
	Xw = (inv_R[0][0]*Xt+inv_R[0][1]*Yt+inv_R[0][2]*Zt)*inv_s_wt - T[0];
	Yw = (inv_R[1][0]*Xt+inv_R[1][1]*Yt+inv_R[1][2]*Zt)*inv_s_wt - T[1];
	Zw = (inv_R[2][0]*Xt+inv_R[2][1]*Yt+inv_R[2][2]*Zt)*inv_s_wt - T[2];
	return true;
}


bool projector::tanDirection2World(float Xt, float Yt, float Zt, float &Xw, float &Yw, float &Zw){
	// X^T = s*R*(X^W+T);
	// X^W = inv(s)inv(R) X^T -T
	Xw = (inv_R[0][0]*Xt+inv_R[0][1]*Yt+inv_R[0][2]*Zt);
	Yw = (inv_R[1][0]*Xt+inv_R[1][1]*Yt+inv_R[1][2]*Zt);
	Zw = (inv_R[2][0]*Xt+inv_R[2][1]*Yt+inv_R[2][2]*Zt);
	return true;
}



bool projector::planePoint2Tan(float a, float b, float &Xt, float &Yt, float &Zt){
	Xt = plane_origin[0]+(a*u[0]+b*v[0])*inv_s_tp;
	Yt = plane_origin[1]+(a*u[1]+b*v[1])*inv_s_tp;
	Zt = plane_origin[2]+(a*u[2]+b*v[2])*inv_s_tp;
	return true;
}


bool projector::planePoint2World(float a, float b, float &X, float &Y, float &Z){
	float Xt,Yt,Zt;
	planePoint2Tan(a,b,Xt,Yt,Zt);
	tanPoint2World(Xt,Yt,Zt,X,Y,Z);
	return true;
}

bool projector::worldPoint2Image(float X, float Y, float Z, float &x, float &y){

	x = P[0][0]*X+P[0][1]*Y+P[0][2]*Z+P[0][3];
	y = P[1][0]*X+P[1][1]*Y+P[1][2]*Z+P[1][3];
	float z = P[2][0]*X+P[2][1]*Y+P[2][2]*Z+P[2][3];
	x = x/z*zy; // zx zy is in matlab coordinate system
	y = y/z*zx; // zx zy is in matlab coordinate system
	// note that here, origin is at the center of the image
	return true;
}



bool projector::planePoint2Image(float xp, float yp, float&x, float&y){;
float zt;
/*PRW constT*/
x = PRW[0]*xp+PRW[3]*yp+constT[0];
y = PRW[1]*xp+PRW[4]*yp+constT[1];
zt = PRW[2]*xp+PRW[5]*yp+constT[2];

/*note that this coordinate system is strange*/
/*origin is at image center, x is left-right and y is top-down*/
x = x/zt*zy;
y = y/zt*zx;
//printf("fast method: x:%f, y:%f,z:%f\n",x,y,zt);
return true;
}


bool projector::planePoint2ImageSlow(float xp, float yp, float &x, float &y){
	float Xt,Yt,Zt;
	float Xw,Yw,Zw;

	//planePoint2Tan(xp,yp,Xt,Yt,Zt);
	Xt = plane_origin[0]+(xp*u[0]+yp*v[0])*inv_s_tp;
	Yt = plane_origin[1]+(xp*u[1]+yp*v[1])*inv_s_tp;
	Zt = plane_origin[2]+(xp*u[2]+yp*v[2])*inv_s_tp;

	//tanPoint2World(Xt,Yt,Zt,Xw,Yw,Zw);
	Xw = (inv_R[0][0]*Xt+inv_R[0][1]*Yt+inv_R[0][2]*Zt)*inv_s_wt - T[0];
	Yw = (inv_R[1][0]*Xt+inv_R[1][1]*Yt+inv_R[1][2]*Zt)*inv_s_wt - T[1];
	Zw = (inv_R[2][0]*Xt+inv_R[2][1]*Yt+inv_R[2][2]*Zt)*inv_s_wt - T[2];


	//worldPoint2Image(Xw,Yw,Zw,x,y);

	x = P[0][0]*Xw+P[0][1]*Yw+P[0][2]*Zw+P[0][3];
	y = P[1][0]*Xw+P[1][1]*Yw+P[1][2]*Zw+P[1][3];
	float z = P[2][0]*Xw+P[2][1]*Yw+P[2][2]*Zw+P[2][3];
	x = x/z*zy; // zx zy is in matlab coordinate system
	y = y/z*zx; // zx zy is in matlab coordinate system
	// note that here, origin is at the center of the image
	//printf("slow method: x:%f, y:%f,z:%f\n",x,y,z);
	return true;
}

bool projector::planeLine2Image(const line &plane_line, line &matlab_line){
	printf("warning, using deprecated function, will do nothing \n");
#if 0
	float s_x,s_y; // start x, y, in Plane coordinate
	float e_x,e_y;

	//float theta = PI*((float)(plane_line.orient)/(n_orient));
	float theta;
	int o = plane_line.orient;
	float len =plane_line.length*0.5f;
	s_x = plane_line.x - len*cache_sin[o];
	s_y = plane_line.y + len*cache_cos[o];

	e_x = plane_line.x + len*cache_sin[o];
	e_y = plane_line.y - len*cache_cos[o];

	float s_x_i,s_y_i;
	float e_x_i,e_y_i;

	planePoint2Image(s_x,s_y,s_x_i,s_y_i);

	planePoint2Image(e_x,e_y,e_x_i,e_y_i);


	len = (s_y_i-e_y_i)*(s_y_i-e_y_i)+(s_x_i-e_x_i)*(s_x_i-e_x_i);
	len = sqrt(len);


	theta = atan((-e_y_i+s_y_i)/(e_x_i-s_x_i)); // this is in image coordinate
	if( fabs(e_x_i - s_x_i)<1e-8)
		theta = PI/2;
	if( theta<0){ theta = theta + PI; }

	matlab_line.x = (int)ROUND(0.5f*(e_x_i+s_x_i)); // this result is in image coordinate,
	matlab_line.y = (int)ROUND(0.5f*(e_y_i+s_y_i));// this result is in image coordinate

	matlab_line.orient = (int)floor(theta/PI*n_orient);
	matlab_line.length = (int)len; /*deprecated, should not be used*/
#endif
	return true;
}



bool projector::planeLineVertex2MatlabImage(const lineVertex &plane_line, lineVertex &matlab_line){

	//planePoint2Image(s_x,s_y,s_x_i,s_y_i);
	/*because reversed argument passing order, matlab_line is origin at center, y: left-right, x top-down*/
	planePoint2Image(plane_line.x_start,plane_line.y_start, matlab_line.y_start,matlab_line.x_start);

	// planePoint2Image(e_x,e_y,e_x_i,e_y_i);
	planePoint2Image(plane_line.x_end,plane_line.y_end, matlab_line.y_end,matlab_line.x_end);

	return true;
}



// decide if current plane is visible under current camera;
bool projector::isVisible(){
	return true;
	float dot = n[0]*cam_dir[0]+n[1]*cam_dir[1]+n[2]*cam_dir[2];
	return dot< -0.4f;
}



void projector::updatePreprocessedMatrix(){

	/*
	FILE* pFile = fopen("./matrix.txt","w");
	fprintf(pFile,"inv_R:\n");
	for( int i = 0; i<3; ++i){
	for(int j=0; j<3; ++j){
	fprintf(pFile,"%f,",inv_R[i][j]);
	}
	printf("\n");
	}

	fprintf(pFile,"inv_s_tp,%f, \t inv_s_wt, %f\n",inv_s_tp,inv_s_wt);
	fprintf(pFile,"po: %f,%f,%f\n",plane_origin[0],plane_origin[1],plane_origin[2]);

	printf("P:\n");
	for( int i = 0; i<3; ++i){
	for(int j=0; j<4; ++j){
	fprintf(pFile,"%f,",P[i][j]);
	}
	fprintf(pFile,"\n");
	}
	*/

	/*W*/
	PRW[0]=u[0];PRW[3]=v[0];
	PRW[1]=u[1];PRW[4]=v[1];
	PRW[2]=u[2];PRW[5]=v[2];
	//fprintf(pFile,"uv:\n %f,%f,\n %f,%f,\n %f,%f\n",PRW[0],PRW[1],PRW[2],PRW[3],PRW[4],PRW[5]);

	/*inv_R*W*/
	float temp[6];
	temp[0]=inv_R[0][0]*PRW[0]+inv_R[0][1]*PRW[1]+inv_R[0][2]*PRW[2];
	temp[1]=inv_R[1][0]*PRW[0]+inv_R[1][1]*PRW[1]+inv_R[1][2]*PRW[2];
	temp[2]=inv_R[2][0]*PRW[0]+inv_R[2][1]*PRW[1]+inv_R[2][2]*PRW[2];

	temp[3]=inv_R[0][0]*PRW[3]+inv_R[0][1]*PRW[4]+inv_R[0][2]*PRW[5];
	temp[4]=inv_R[1][0]*PRW[3]+inv_R[1][1]*PRW[4]+inv_R[1][2]*PRW[5];
	temp[5]=inv_R[2][0]*PRW[3]+inv_R[2][1]*PRW[4]+inv_R[2][2]*PRW[5];

	//fprintf(pFile,"Temp:\n %f,%f,\n %f,%f,\n %f,%f\n",temp[0],temp[1],temp[2],temp[3],temp[4],temp[5]);




	/*P33*inv_R*W*/
	PRW[0] = P[0][0]*temp[0]+P[0][1]*temp[1]+P[0][2]*temp[2];
	PRW[1] = P[1][0]*temp[0]+P[1][1]*temp[1]+P[1][2]*temp[2];
	PRW[2] = P[2][0]*temp[0]+P[2][1]*temp[1]+P[2][2]*temp[2];

	PRW[3] = P[0][0]*temp[3]+P[0][1]*temp[4]+P[0][2]*temp[5];
	PRW[4] = P[1][0]*temp[3]+P[1][1]*temp[4]+P[1][2]*temp[5];
	PRW[5] = P[2][0]*temp[3]+P[2][1]*temp[4]+P[2][2]*temp[5];
	/*alpha*beta*P33*inv_R*W*/
	float ab = inv_s_tp*inv_s_wt;
	for(int i =0; i<6; ++i)	PRW[i]*=ab;

	//printf("PRW[5]:%f,!!!!!!!!!!!\n",PRW[5]);
	//fprintf(pFile,"PRW:\n %f,%f,\n %f,%f,\n %f,%f\n",PRW[0],PRW[1],PRW[2],PRW[3],PRW[4],PRW[5]);

	/*constT*/
	/*constT=inv_R*po*/
	constT[0]=inv_R[0][0]*plane_origin[0]+inv_R[0][1]*plane_origin[1]+inv_R[0][2]*plane_origin[2];
	constT[1]=inv_R[1][0]*plane_origin[0]+inv_R[1][1]*plane_origin[1]+inv_R[1][2]*plane_origin[2];
	constT[2]=inv_R[2][0]*plane_origin[0]+inv_R[2][1]*plane_origin[1]+inv_R[2][2]*plane_origin[2];

	/**beta*inv_R*po-T*/
	for( int i =0; i<3; ++i)
		constT[i]=constT[i]*inv_s_wt-T[i];

	/**P33(beta*inv_R*po-T)*/
	temp[0] = P[0][0]*constT[0]+P[0][1]*constT[1]+P[0][2]*constT[2];
	temp[1] = P[1][0]*constT[0]+P[1][1]*constT[1]+P[1][2]*constT[2];
	temp[2] = P[2][0]*constT[0]+P[2][1]*constT[1]+P[2][2]*constT[2];

	/**P31+P33(beta*inv_R*po-T)*/
	for( int i =0; i<3; ++i)
		constT[i]=P[i][3]+temp[i];

	/*
	fprintf(pFile,"T:\n %f,%f,%f,\n",T[0],T[1],T[2]);
	fprintf(pFile,"constT:\n %f,%f,%f,\n",constT[0],constT[1],constT[2]);
	fclose(pFile);
	*/
}




 bool projector::estimatePandDir(const std::deque<pt_pair_s> &ptps,float dest_P[][4], float dir[3], float &error){
	int n_pts= ptps.size();
	if(n_pts<4){
		printf("Not sufficient number of points\n");
		for(int r = 0; r<3 ;++r){
			for(int c=0; c<4; ++c){
				dest_P[r][c]=0.f;
			}
			dir[r]=0.f;
		}
		dir[2]=1.0f;
		dest_P[2][3]=1.0f;
		return false;
	}else if( n_pts<=8){
		estimateOrthognalTransform(ptps,dest_P,dir,error);
	}else{
		estimatePerspectiveTransform(ptps,dest_P,dir,error);
	}


	return true;
}


 bool projector::estimatePerspectiveTransform(const std::deque<pt_pair_s> &ptps, float dest_P[][4],float dir[3], float&error){

	int n_pt = ptps.size();

#ifndef USE_OPEN_CV 
	Eigen::MatrixXf dlt(2*n_pt,12);

	for( int i_pt = 0; i_pt<n_pt; ++i_pt){
		const pt_pair_s &ref_pair = ptps[i_pt];
		dlt.row(2*i_pt)<< 0.0f,0.0f,0.0f,0.0f,-1*ref_pair[0],-1*ref_pair[1],-1*ref_pair[2],-1,ref_pair[4]*ref_pair[0],ref_pair[4]*ref_pair[1],ref_pair[4]*ref_pair[2],ref_pair[4];
		dlt.row(2*i_pt+1)<<1*ref_pair[0],1*ref_pair[1],1*ref_pair[2],1.f,0.f,0.f,0.f,0.f,-ref_pair[3]*ref_pair[0],-ref_pair[3]*ref_pair[1],-ref_pair[3]*ref_pair[2],-ref_pair[3];
	}

	Eigen::JacobiSVD<Eigen::MatrixXf> svd(dlt, Eigen::ComputeFullV);/*second parameter means full V*/
	svd.computeV();
	Eigen::MatrixXf V = svd.matrixV();
	int nzero = svd.nonzeroSingularValues();
	Eigen::MatrixXf resP = V.col(nzero-1);
	resP.resize(4,3);
	resP.transposeInPlace();
	error = 0;
	for(int i_pt = 0; i_pt<n_pt; ++i_pt){
		Eigen::Vector4f pt_3D;
		const pt_pair_s &ref_pair = ptps[i_pt];
		pt_3D<<ref_pair[0],ref_pair[1],ref_pair[2],1.0f;
		Eigen::Vector3f res_2D = resP*pt_3D;
		float dx  = res_2D(0)/res_2D(2)-ref_pair[3];
		float dy = res_2D(1)/res_2D(2)-ref_pair[4];
		float e_pt = dx*dx+dy*dy;
		error +=sqrt(e_pt);
	}
	error /=n_pt;

	for( int row = 0; row<3; ++row){
		for( int col = 0; col<4; ++col){
			dest_P[row][col]=resP(row,col);
		}
	}

	/*now cam direction*/
	Eigen::Vector3f cam_dir_vec;
	projector::cam_direction(resP,cam_dir_vec);	
	for( int i_el = 0;i_el<3;++i_el){
		dir[i_el]=cam_dir_vec(i_el);
	}
	std::cout<<"P"<<std::endl<<resP<<std::endl;
	std::cout<<"cam_dir"<<std::endl<<cam_dir_vec<<std::endl;
#else
	float resP[3][4];
	cv::Mat obj_pts(3,n_pt,CV_32FC1);
	cv::Mat img_pts(2,n_pt,CV_32FC1);

	cv::Mat f(3,1,CV_32FC1);
	f.at<float>(0)=580; f.at<float>(1)=580;f.at<float>(2)=1;
	cv::Mat K=cv::Mat::diag(f);

	for(int i_pt =0; i_pt<n_pt; ++i_pt){
		obj_pts.at<float>(0,i_pt)=ptps[i_pt][0];
		obj_pts.at<float>(1,i_pt)=ptps[i_pt][1];
		obj_pts.at<float>(2,i_pt)=ptps[i_pt][2];

		img_pts.at<float>(0,i_pt)=ptps[i_pt][3];
		img_pts.at<float>(1,i_pt)=ptps[i_pt][4];
	}



	cv::Mat rVec,tVec;
	cv::Mat dis(0,0,CV_32FC1);
	cv::Mat obj_pts_t = obj_pts.t();
	cv::Mat img_pts_t = img_pts.t();

	//std::cout<<"obj:\n"<<obj_pts_t<<std::endl;
	//std::cout<<"img:\n"<<obj_pts_t<<std::endl;
	
	cv::solvePnP(obj_pts_t,img_pts_t,K,dis,rVec,tVec);
	cv::Mat Rdouble(3,3,CV_64FC1);
	cv::Rodrigues(rVec,Rdouble);
	cv::Mat R;
	Rdouble.convertTo(R,CV_32FC1);

	//std::cout<<"Rdouble:\n"<<Rdouble<<std::endl;
	//std::cout<<"R:\n"<<R<<std::endl;
	//std::cout<<"K:\n"<<K<<std::endl;


	cv::Mat KR=K*R;
	cv::Mat tVecSingle;
	tVec.convertTo(tVecSingle,CV_32FC1);
	cv::Mat KT=K*tVecSingle;


	//std::cout<<"KR:\n"<<KR<<std::endl;
	//std::cout<<"KT:\n"<<KT<<std::endl;
	//std::cout<<"estP\n"<<estP<<std::endl;
	error = 0; 
	for(int i_pt =0; i_pt<n_pt; ++i_pt){
		cv::Mat proj_img_pt = KR*obj_pts.col(i_pt)+KT;
		proj_img_pt /=proj_img_pt.at<float>(2);
		float dx = proj_img_pt.at<float>(0)-img_pts.at<float>(0,i_pt);
		float dy = proj_img_pt.at<float>(1)-img_pts.at<float>(1,i_pt);
		//printf(" error: %f\n",sqrt(dx*dx+dy*dy));
		error += sqrt(dx*dx + dy*dy);
	}

	error/=n_pt;
	for( int row = 0; row<3; ++row){
		for( int col = 0; col<3; ++col){
			dest_P[row][col]=KR.at<float>(row,col);
		}
		dest_P[row][3]=KT.at<float>(row);
	}
	cam_direction(dest_P,dir);
	
#endif
	
	



	return true;
}


 bool projector::cam_direction(float srcP[3][4], float cam_dir[3]){
	 Eigen::MatrixXf Pmat(3,4); 
	 Eigen::Vector3f dir(3);
	 for(int row = 0; row<3; ++row){
		for(int col =0; col<4; ++col){
			Pmat(row,col)=srcP[row][col];
		}
		}
	 if(! projector::cam_direction(Pmat,dir)) return false;
	 for(int col =0; col<3; ++col) cam_dir[col]=dir(col);

	return true;
 }

 bool projector::cam_direction(Eigen::MatrixXf Pmat, Eigen::Vector3f &cam_dir){

	Eigen::MatrixXf core = Pmat.block<3,3>(0,0);

	core.transposeInPlace();
	core.reverseInPlace();

	Eigen::MatrixXf QR=core.householderQr().matrixQR();
	Eigen::MatrixXf K(3,3); 
	K.setZero();
	for( int i = 0; i<3; ++i){
		for(int j = i; j<3 ; ++j){
			K(i,j)=QR(i,j);
		}
	}
	
	K.transposeInPlace();
	K.reverseInPlace();

	core.reverseInPlace();
	core.transposeInPlace();
	Eigen::MatrixXf R = K.inverse()*core;

	if(R.determinant()<0){
		K.col(0)*=-1.0;
		R.row(0)*=-1.0;
	}
	/*end of vgg_RQ(core)*/
	float ratio = K(2,2);
	K = K/ratio;
	if( K(0,0)<0){
		//Eigen::MatrixXd D = Eigen::MatrixXd::Zero(3);
		Eigen::MatrixXf D(3,3);
		D.setZero();
		D(0,0) = -1; D(1,1)=-1; D(2,2)=1;
		K = K * D;
		R = D * R;
	}
	//Eigen::MatrixXf I = R * R.transpose();
	//std::cout<<"I\n"<<I<<std::endl;
	//assert(I.isIdentity(1e-4));


	/*Eigen::Vector3f  z_axis;
	z_axis<<0.0f,0.0f,1.0f;
	cam_dir = R.transpose()*z_axis;*/
	R.row(2).normalize();
	cam_dir = R.row(2);
	return true;
}

